#include "stdafx.h"


#include	<math.h>	
#include	"WGL_ARTK.h"



cWglARTK::cWglARTK( void ) : m_pARTImage( NULL ), m_pArglSettings( NULL ), m_bMarkerFound( false )
{
}


cWglARTK::~cWglARTK( void )
{
	cleanup();
}

// initialize the AR
bool cWglARTK::initAR(	   const char *szCameraParamFName, 
						   const char *szPattFName,
						   const char *szVConfFName,
						   int *pImgWidth,
						   int *pImgHeight)
{

	ARParam			sCamParamTemp;
	int				iCamImgSizeX,
					iCamImgSizeY;

	/* open the video path */
	if( arVideoOpen( const_cast<char*>(szVConfFName) ) < 0 )
	{
		MessageBox(NULL, L"Unable to open connection to camera.\n", L"ERROR", MB_OK );
		return false;
	}

	/* find the size of the window */
	if( arVideoInqSize( &iCamImgSizeX, &iCamImgSizeY ) < 0 ) return false;

	/* set the initial camera parameters */
	if( arParamLoad( szCameraParamFName, 1, &sCamParamTemp ) < 0 )
	{
		MessageBox(NULL, L"Error loading parameter file for camera.\n", L"ERROR", MB_OK );
		return false;
	}

	arParamChangeSize( &sCamParamTemp, iCamImgSizeX, iCamImgSizeY, &m_sCameraParam );

	arInitCparam( &m_sCameraParam );

	/* start capture image */
	if( arVideoCapStart() != 0 )
	{
		MessageBox(NULL, L"Unable to begin camera data capture.\n", L"ERROR", MB_OK );
		return false;
	}

    if( (m_iPattID = arLoadPatt( szPattFName )) < 0 )
    {
		MessageBox(NULL, L"Pattern file load error !!\n", L"ERROR", MB_OK );
		return false;
    }

	m_dViewScaleFactor = 0.16;
	m_dPattWidth = 40.0;
	m_bFirstTime = true;

	*pImgWidth = iCamImgSizeX;
	*pImgHeight = iCamImgSizeY;

	return true;
}


bool cWglARTK::initArgl( void )
{
	// Initialise the gsub_lite library for the current OpenGL context.
	// windows use current OPENGL context to draw image
	if( (m_pArglSettings = arglSetupForCurrentContext()) == NULL )
	{
		MessageBox(NULL,  L"arglSetupForCurrentContext() returned error.\n" , L"ERROR", MB_OK);
		return false;
	}

	double	c = cos( 3.1515926 * 0.5 );
	double	s = sin( 3.1515926 * 0.5 );
	m_dmatRotX[ 0] = 1.0; m_dmatRotX[ 1] = 0.0; m_dmatRotX[ 2] = 0.0; m_dmatRotX[ 3] = 0.0;
	m_dmatRotX[ 4] = 0.0; m_dmatRotX[ 5] =   c; m_dmatRotX[ 6] =   s; m_dmatRotX[ 7] = 0.0;
	m_dmatRotX[ 8] = 0.0; m_dmatRotX[ 9] =  -s; m_dmatRotX[10] =   c; m_dmatRotX[11] = 0.0;
	m_dmatRotX[12] = 0.0; m_dmatRotX[13] = 0.0; m_dmatRotX[14] = 0.0; m_dmatRotX[15] = 1.0;

	// rotate 90 degree alone x axises 
	MatrixRotationX( m_matRotX, -3.1415926f * 0.5f );

	return true;
}


// if we have found a marker, then return true, otherwise false
bool cWglARTK::update( void )
{
	ARUint8			*pImage;
	ARMarkerInfo	*pMarkerInfo;
    int             iNumDetectedMarker;

	// capture the next image
	if( (pImage = arVideoGetImage()) != NULL )
	{
		m_pARTImage = pImage;
		m_bMarkerFound = false;

		// detect marker
		if( arDetectMarker( m_pARTImage, 130, &pMarkerInfo, &iNumDetectedMarker ) < 0 )
		{
			exit( -1 );
		}

		// check which marker is detected
		int		k = -1;
		for( int j = 0 ; j < iNumDetectedMarker ; j++ )
		{
			if( pMarkerInfo[j].id == m_iPattID )
			{
				if( k == -1 || pMarkerInfo[j].cf > pMarkerInfo[k].cf )	k = j;
			}
		}

		// we have found sth
		if( k != -1 )
		{
			/* get the transformation between the marker and the real camera */
			if( m_bFirstTime )
				arGetTransMat( &(pMarkerInfo[k]), m_dPattCenter, m_dPattWidth, m_dPattTransMat );
			else
				arGetTransMatCont( &(pMarkerInfo[k]), m_dPattTransMat, m_dPattCenter, m_dPattWidth, m_dPattTransMat );

			m_bFirstTime = false;

			m_bMarkerFound = true;
		}

		return true;
	}

	arUtilSleep( 2 );

	return false;
}

// show image
void cWglARTK::display( void )
{
	// draw image to the current context (remember? we have called arglSetupForCurrentContext before)
	arglDispImage( m_pARTImage, &m_sCameraParam, 1.0, m_pArglSettings );	// zoom = 1.0.
}

void cWglARTK::capNextImage( void )
{
	arVideoCapNext();

	m_pARTImage = NULL; // arVideoCapNext() will disable the buffer
}

void cWglARTK::getCameraPos( Vector3 *pvec3CamPos )
{
	double		dInvPattTransMat[3][4];

	// make the inverse of the source matrix to the destination one
	arUtilMatInv( m_dPattTransMat, dInvPattTransMat );

	pvec3CamPos->x = (float)dInvPattTransMat[0][3];
	pvec3CamPos->y = (float)dInvPattTransMat[1][3];
	pvec3CamPos->z = (float)dInvPattTransMat[2][3];
	Vector3Transform( pvec3CamPos, pvec3CamPos, m_matRotX );

	pvec3CamPos->x *= (float)m_dViewScaleFactor;
	pvec3CamPos->y *= (float)m_dViewScaleFactor;
	pvec3CamPos->z *= (float)m_dViewScaleFactor;
}



void cWglARTK::getProjectionMat( double dMat[16] )
{
								//        near  far
	arglCameraFrustumRH( &m_sCameraParam, 0.1, 400.0, dMat );
}


void cWglARTK::getModelViewMat( double dMat[16] )
{
	double	dmatTemp[16];

	arglCameraViewRH( m_dPattTransMat, dmatTemp, m_dViewScaleFactor );

	for( int i = 0 ; i < 4 ; i++ )
	{
		int		idx = i << 2;
		dMat[idx + 0] =	m_dmatRotX[idx] * dmatTemp[0] + m_dmatRotX[idx + 1] * dmatTemp[4] + m_dmatRotX[idx + 2] * dmatTemp[ 8] + m_dmatRotX[idx + 3] * dmatTemp[12];
		dMat[idx + 1] =	m_dmatRotX[idx] * dmatTemp[1] + m_dmatRotX[idx + 1] * dmatTemp[5] + m_dmatRotX[idx + 2] * dmatTemp[ 9] + m_dmatRotX[idx + 3] * dmatTemp[13];
		dMat[idx + 2] =	m_dmatRotX[idx] * dmatTemp[2] + m_dmatRotX[idx + 1] * dmatTemp[6] + m_dmatRotX[idx + 2] * dmatTemp[10] + m_dmatRotX[idx + 3] * dmatTemp[14];
		dMat[idx + 3] =	m_dmatRotX[idx] * dmatTemp[3] + m_dmatRotX[idx + 1] * dmatTemp[7] + m_dmatRotX[idx + 2] * dmatTemp[11] + m_dmatRotX[idx + 3] * dmatTemp[15];
	}
}

// change the scale factor
void cWglARTK::addViewScaleFactor( double dAdd )
{
	m_dViewScaleFactor += dAdd;

	if( m_dViewScaleFactor < 0.01 )	m_dViewScaleFactor = 0.01;
	if( 0.80 < m_dViewScaleFactor )	m_dViewScaleFactor = 0.80;
}

// clear everything
void cWglARTK::cleanup( void )
{
	if( m_pArglSettings )
	{
		arglCleanup( m_pArglSettings );
		m_pArglSettings = NULL;
	}
	arVideoCapStop();
	arVideoClose();
}
